#include <math.h>
#include <cmath>
#include <iostream>
#include <fstream>
#include <iostream>
#include <sstream>

#define NUM_PART 200
#define I_X 0
#define I_Y 1
#define I_XVEL 2
#define I_YVEL 3
#define PART 4
#define RADIUS 3
#define DELTAT 0.0002

#define GRID_SCALE 1.
int countParticles(double cx, double cy, double r);
void fillParticleCircle(double cx, double cy, double r, double* particles, int offset, double startVelocityX, double startVelocityY);
inline void simulationStep(double *in, double *out, const int numParticles);
void ausgabe(double *particles, int offset, int timeStamp);
int main()
{
	int offset2ndCircle = countParticles(0., 0., RADIUS);
	int numParticlesFirstCircle = offset2ndCircle;
	offset2ndCircle *= PART;
	
	const int numParticles = numParticlesFirstCircle + countParticles(0., 0., RADIUS);
	double* particles = new double[2*numParticles*PART];

	fillParticleCircle(0., 0., RADIUS, particles, 0, 0, 0);
	fillParticleCircle(14., -4.5, RADIUS, particles, offset2ndCircle, -10, 0);
	ausgabe(particles, offset2ndCircle, 0);
	simulationStep(particles, &particles[numParticles*PART], numParticles);
	ausgabe(particles, offset2ndCircle, 1);
	int count = 0;
	for(double t=0 ; t<0.02 ; t+=DELTAT)
	{
		simulationStep(particles, &particles[numParticles], numParticles);
		simulationStep(&particles[numParticles], particles, numParticles);
		//if(count%100 == 0 && count > 0)
			ausgabe(particles, offset2ndCircle, count);
		count++;
	}
	delete [] particles;
	std::cout << "Fertsch!";
	std::cin.get();
}

int countParticles(double cx, double cy, double r)
{
	int result=0;
	const double deltaX = 0.4;
	const double rxr = r*r;

	for(double x=-(r+fmod(r,deltaX)) ; x<r+fmod(r,deltaX) ; x+=deltaX)
	{
		for(double y=-(r+fmod(r,deltaX)) ; y<r+fmod(r,deltaX) ; y+=deltaX)
		{
			result += x*x+y*y<rxr?1:0;
		}
	}
	return result;
}

void fillParticleCircle(double cx, double cy, double r, double* particles, int offset, double startVelocityX, double startVelocityY)
{
	const double deltaX = 0.4;
	const double rxr = r*r;
	for(double x=-(r+fmod(r,deltaX)) ; x<r+fmod(r,deltaX) ; x+=deltaX)
	{
		for(double y=-(r+fmod(r,deltaX)) ; y<r+fmod(r,deltaX) ; y+=deltaX)
		{
			if(x*x+y*y<rxr)
			{
				particles[offset + I_X] = x+cx;
				particles[offset + I_Y] = y+cy;
				particles[offset + I_XVEL] = startVelocityX;
				particles[offset + I_YVEL] = startVelocityY;
				offset+=PART;
			}
		}
	}
}

inline void simulationStep(double *startZustand, double *folgeZustand, const int numParticles)
{
	const double g = 9.81;
	int partikelI, partikelJ;
	double z, rQuadrat, ki, kopplungX, kopplungY, summeFX, summeFY;
	std::fstream file;
	file.open("Zwischenschritte.txt", std::ios::out | std::ios::binary | std::ios::app);
	file << "Startzustand X\tStartzustand X-Velocity\tStartzustand Y\tStartzustand Y-Velocity\n";
	for(int i = 0; i < numParticles; i+=4)
	{
		kopplungX = 0;
		kopplungY = 0;
		summeFX = 0;
		summeFY = 0;
		ki = 0;
		for(int j = 0; j < numParticles; j+=4)
		{
			rQuadrat = startZustand[j+I_X]*startZustand[j+I_Y] - startZustand[i+I_X]*startZustand[i+I_Y];
			if(!rQuadrat == 0)
			{
				ki += (20*rQuadrat-8)/(rQuadrat*rQuadrat*rQuadrat);
				summeFX += ki*startZustand[j+I_X]-startZustand[i+I_X];
				summeFY += ki*startZustand[j+I_Y]-startZustand[i+I_Y];
			}
		}
		kopplungX = summeFX;
		kopplungY = g+summeFY;
		//Schritt 1
		folgeZustand[i+I_XVEL] = startZustand[i+I_XVEL]+DELTAT/2*kopplungX;
		folgeZustand[i+I_YVEL] = startZustand[i+I_YVEL]+DELTAT/2*kopplungY;
		folgeZustand[i+I_XVEL] = startZustand[i]+DELTAT*folgeZustand[i+I_XVEL];
		folgeZustand[i+I_YVEL] = startZustand[i]+DELTAT*folgeZustand[i+I_YVEL];
		if(i != 0)
		{
			folgeZustand[i+I_X] = startZustand[i+I_X]+DELTAT*startZustand[i+I_XVEL];
			folgeZustand[i+I_Y] = startZustand[i+I_Y]+DELTAT*startZustand[i+I_YVEL];

			file << startZustand[i+I_X] << "\t" << startZustand[i+I_XVEL] << "\t" << startZustand[i+I_Y] << "\t" << startZustand[i+I_YVEL] << std::endl;
		}
		else
		{
			folgeZustand[i+I_X] = startZustand[i+I_X];
			folgeZustand[i+I_Y] = startZustand[i+I_Y];
		}

	}
	file << "\n\n";
	file.close();

	/*
	const double gravity = -9.81;
	double dist;
	double xdist, ydist;
	int parti, partj;
	for(int i=0 ; i<numParticles ; ++i)
	{
		out[i*PART+I_XVEL] = 0;
		out[i*PART+I_YVEL] = 0;
	}
	for(int i=0 ; i<numParticles ; ++i)
	{
		parti = i*PART;
		for(int j=0 ; j<(i-1) ; ++j)
		{
			partj = j*PART;
			xdist = in[parti+I_X]-in[partj+I_X];
			ydist = in[parti+I_Y]-in[partj+I_Y];

			dist = xdist*xdist+ydist*ydist;
			dist = ((20.0*dist-8.0)/(dist*dist*dist))*dt;
			out[parti+I_XVEL] += dist*xdist;
			out[partj+I_XVEL] -= dist*xdist;
			out[parti+I_YVEL] += dist*ydist;
			out[partj+I_YVEL] -= dist*ydist;
		}
		out[parti+I_X] += out[parti+I_XVEL]*dt;
		out[parti+I_Y] += out[parti+I_YVEL]*dt;
	}
	*/
}

void ausgabe(double *particles, int offset, int timeStamp)
{
	std::fstream file;
	std::ostringstream oss;
	oss << "PartikelAusgabe" << " " << timeStamp << ".txt";
	std::string ausgabe = oss.str();
	file.open(ausgabe.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
	file << "Tropfen1 X\t Tropfen1 Y\tTropfen2 X\t Tropfen2 Y\n";
	for(int i = 0; i <= offset-PART; i+=PART)
		file << particles[i] << "\t" << particles[i+1] << "\t" << particles[offset+i] << "\t" << particles[offset+i+1] << std::endl;
	file.close();
	std::cout << "Step " << ausgabe.c_str() << std::endl;
}